#pragma once

#include "vio/vio_common.h"

namespace vio {
class Camera {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef std::shared_ptr<Camera> Ptr;

  Camera();

  Camera(const int& width,
         const int& height,
         const Eigen::Matrix3d& K_in,
         const Eigen::Matrix3d& K_out,
         const Eigen::Vector4d& D)
      : width_(width), height_(height), K_in_(K_in), K_out_(K_out), D_(D) {
    fx_ = K_out_(0, 0);
    cx_ = K_out_(0, 2);
    fy_ = K_out_(1, 1);
    cy_ = K_out_(1, 2);
    K_in_inv_ = K_in_.inverse();
  }

  Eigen::Matrix3d K() const {
    return K_out_;
  }

  cv::Mat cvK() const {
    return (
        cv::Mat_<double>(3, 3) << fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0);
  }

  // projection and unprojection
  Eigen::Vector3d KB_unprojection(const Eigen::Vector3d& p2);
  Eigen::Vector2d KB_projection(const Eigen::Vector3d& p3_cam);

  cv::Point3f KB_unprojection(const cv::Point2f& p2);
  cv::Point2f KB_projection(const cv::Point3f& p3_cam);

  void undistortImage(const cv::Mat& img, cv::Mat& out_img);

  // frame transform: world, camera, pixel
  Eigen::Vector3d world2camera(const Eigen::Vector3d& p_w,
                               const Sophus::SE3d& T_c_w);

  Eigen::Vector3d camera2world(const Eigen::Vector3d& p_c,
                               const Sophus::SE3d& T_c_w);

  Eigen::Vector2d camera2pixel(const Eigen::Vector3d& p_c);
  cv::Point2f camera2pixel(const cv::Point3f& p_c);

  Eigen::Vector3d pixel2camera(const Eigen::Vector2d& p_p, double depth = 1);
  cv::Point3f pixel2camera(const cv::Point2f& p_p, double depth = 1);

  Eigen::Vector3d pixel2world(const Eigen::Vector2d& p_p,
                              const Sophus::SE3d& T_c_w,
                              double depth = 1);

  Eigen::Vector2d world2pixel(const Eigen::Vector3d& p_w,
                              const Sophus::SE3d& T_c_w);

  inline int width() const {
    return width_;
  }

  inline int height() const {
    return height_;
  }

  inline bool IsInFrame(const Eigen::Vector2i& obs, int boundary = 0) const {
    if (obs[0] >= boundary && obs[0] < width() - boundary &&
        obs[1] >= boundary && obs[1] < height() - boundary)
      return true;
    return false;
  }

  inline bool IsInFrame(const Eigen::Vector2i& obs,
                        int boundary,
                        int level) const {
    if (obs[0] >= boundary && obs[0] < width() / (1 << level) - boundary &&
        obs[1] >= boundary && obs[1] < height() / (1 << level) - boundary)
      return true;
    return false;
  }

  const Eigen::Vector2d FocalLength() const {
    return Eigen::Vector2d(fx_, fy_);
  }

  virtual double ErrorMultiplier2() const {
    return std::fabs(fx_);
  }

  virtual double ErrorMultiplier() const {
    return std::fabs(4.0 * fx_ * fy_);
  }

 public:
  // image info
  int width_;
  int height_;
  // camera calibration parameters
  Eigen::Matrix3d K_in_;  // pixel to camera frame (input image)
  Eigen::Matrix3d K_in_inv_;
  Eigen::Matrix3d K_out_;  // pixel to camera frame (output image)
  Eigen::Vector4d D_;      // kannala_brandt or equidistant distortion models
  double fx_;
  double fy_;
  double cx_;
  double cy_;
};
}  // namespace vio